clear all
close all
N = 50; % number of sample points
tf = 1; % final time
dt = tf/N; % sample time
%%% Constraints
xi = [0,0]'; % initial constraint
xf = [0,0]'; % final constraint
n = 3*(N+1);
Aeq = zeros(4, n); 
Aeq(1:2,1:2) = eye(2); 
%Aeq(3:4,end-2:end-1) = eye(2);
Beq = [xi; xf];
%%% Start guess
X0 = [xi; 0]*ones(1,N+1);
X0 = X0(:);

options = optimset('fmincon');
options = optimset(options, 'Algorithm','interior-point');
options = optimset(options, 'GradObj','off');
options = optimset(options, 'GradConstr','off');
[X,fval] = fmincon(@ZermeloCost, X0, [], [], ...
Aeq, Beq, [], [], @ZermeloCon, options, dt);

%%% Show the result
fprintf('Final value of the objective function: %0.6f \n', fval)
tt = dt*(0:N);
x = X(1:3:end)';
y = X(2:3:end)';
u = X(3:3:end)';
figure
subplot(2,1,1), plot( tt, x, 'b.-', tt, y, 'g+-'), xlabel('t'), legend('x','y')
title('Second Order System state variables');
subplot(2,1,2), plot( tt(1:end-1), u(1:end-1), 'b+-'), xlabel('t'), ylabel('u')
title('Second Order System control');